﻿/*
 * @Author: Guo Xuemei 1242143575@qq.com
 * @Date: 2023-02-08 15:32:17
 * @LastEditors: Guo Xuemei 1242143575@qq.com
 * @LastEditTime: 2023-04-06 11:08:23
 * @FilePath: /vanjee_lidar_sdk_test/src/vanjee_lidar_sdk/src/vanjee_driver/src/vanjee_driver/driver/decoder/decoder_vanjee740.hpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AEnam
 */
#pragma once 

#include <vanjee_driver/driver/decoder/decoder_mech.hpp>
#include <vanjee_driver/driver/difop/protocol_base.hpp>
#include <vanjee_driver/driver/difop/cmd_class.hpp>
#include <vanjee_driver/driver/difop/protocol_abstract.hpp>
#include <vanjee_driver/driver/decoder/wlr740/protocol/frame/cmd_repository_740.hpp>

#include <vanjee_driver/driver/decoder/wlr740/decoder/params_scan_data.hpp>
#include <vanjee_driver/driver/decoder/wlr740/decoder/one_point_info_manage.hpp>
#include <vanjee_driver/driver/decoder/wlr740/algorithm/filterate.hpp>

#ifdef ENABLE_TRANSFORM

#include <Eigen/Dense>
#define POINTS_COMPENSE true
#define POINTCLOUD_COMPENSE false
#define PKG_POINT_COMPENSE false
#define SINGLE_POINT_COMPENSE true

#endif

constexpr unsigned int ONE_CIRCLE_CLOUDS = (144*3600);

namespace vanjee
{
namespace lidar
{
  #pragma pack(push,1)

  typedef struct  
  {
    uint8 time_flag[2];
    uint32 microsecond;
    uint32 second;
    uint8 reserved[2];
  }NtpTime;

  typedef struct
  {
    uint16 distance;
    uint8 intensity;
  }ChannelData0728;

  typedef struct
  {
    uint16 distance;
    uint8 intensity;
    uint16 low_thres_pulse_width;
    uint16 high_thres_pulse_width;
  }ChannelData0728WithPulseWidth;

  typedef struct 
  {
    uint8 header;
    uint8 rotate_mirror_id;
    uint16 horizontal_angle;
    uint8 horizontal_angle_res;
    uint8 timing_thres_type;
    uint8 echo_type;
    int8 vertical_angle_compensation;
    int32 horizontal_angle_compensation;
    ChannelData0728 channel_data[48];
  }DataBlock0728;

  typedef struct 
  {
    uint8 header;
    uint8 rotate_mirror_id;
    uint16 horizontal_angle;
    uint8 horizontal_angle_res;
    uint8 timing_thres_type;
    uint8 echo_type;
    int8 vertical_angle_compensation;
    int32 horizontal_angle_compensation;
    ChannelData0728WithPulseWidth channel_data[48];
    uint8 strong_weak_flag[6];
  }DataBlock0728WithPulseWidth;
  
  typedef struct 
  {
    uint8 header[2];
    uint16 frame_len;
    uint8 device_type[2];
    uint8 frame_type[2];
    union{
      uint8 time_info[12];
      NtpTime ntp_time;
    }time_field;
    uint16 frame_index;
    uint8 protocol_version[4];
    uint16 horizontal_angle_offset;
  }Vanjee740Difop;

  typedef struct 
  {
    // x方向补偿位移
    uint16_t distance_offset_x;
    // y方向补偿位移
    uint16_t distance_offset_y;
    // z方向补偿位移
    uint16_t distance_offset_z;
    // 四元数w值
    float quaternion_w;
    // 四元数x值
    float quaternion_x;
    // 四元数y值
    float quaternion_y;
    // 四元数z值
    float quaternion_z;
  }Vanjee740DataIMU;

  typedef struct 
  {
    Vanjee740Difop difop;
    DataBlock0728 data_block[8];
  }Vanjee740MsopPkt0728;

  typedef struct 
  {
    Vanjee740Difop difop;
    DataBlock0728WithPulseWidth data_block[4];
  }Vanjee740MsopPkt0728WithPulseWidth;

  typedef struct 
  {
    Vanjee740Difop difop;
    DataBlock0728 data_block[8];
    Vanjee740DataIMU data_imu;
  }Vanjee740MsopPkt1122;

  typedef struct 
  {
    Vanjee740Difop difop;
    DataBlock0728WithPulseWidth data_block[4];
    Vanjee740DataIMU data_imu;
  }Vanjee740MsopPkt1122WithPulseWidth;

  #pragma pack(pop)

    template <typename T_PointCloud>
    class DecoderVanjee740 :public DecoderMech<T_PointCloud>
    {
    private:
        std::shared_ptr<SplitStrategy> split_strategy_; 
        static WJDecoderMechConstParam& getConstParam(uint8_t mode);
        static WJEchoMode getEchoMode(uint8_t mode);
        OnePointInfoManage* m_qmap_one_circle_point_ = new OnePointInfoManage();
        OnePointInfoManage* m_qmap_pcap_one_circle_point_ = new OnePointInfoManage();
        bool algorithm_enable_ = true;
        bool blank_point_compen_enable_ = true;
        bool outlier_enable_ = true;
        bool close_range_outliers_enable_ = true;

        int32_t pre_frame_id_ = -1;

        int h_angle_lum_comp_[48] = {1,3,5,7,2,4,6,8, 
                                        1,3,5,7,2,4,6,8,
                                        1,3,5,7,2,4,6,8,
                                        1,3,5,7,2,4,6,8,
                                        1,3,5,7,2,4,6,8,
                                        1,3,5,7,2,4,6,8};

        int rotation_rate_ = 600; // 600转 / 分
        double h_angle_1_6us_ = ((360.0 / ((1000.0 / (rotation_rate_ / 60.0)) * 1000.0)) * 1.6 * 2) * 1000; // 1.6us转多少角度 精度 0.001 * 1000
#ifdef ENABLE_TRANSFORM
        void pointsCompense(OnePointInfoManage &points_info, const Eigen::Quaterniond q, const Eigen::Vector3d pos_t);
        void pkgPointsCompense(OnePointInfoManage &points_info, const Eigen::Quaterniond q, const Eigen::Vector3d pos_t, int block_num);
        void pointCompense(OnePointInfo* point_info, const Eigen::Quaterniond q, const Eigen::Vector3d pos_t);
#endif
    public:
        constexpr static double FRAME_DURATION = 0.1;
        constexpr static uint32_t SINGLE_PKT_NUM = 450;

        virtual bool decodeMsopPkt(const uint8_t* pkt, size_t size);
        virtual void processDifopPkt(std::shared_ptr<ProtocolBase> protocol);
        virtual ~DecoderVanjee740() = default;
        explicit DecoderVanjee740(const WJDecoderParam& param);

        inline uint32 GetOnePointInfoKey(const OnePointInfo& val);
        
        bool decodeMsopPkt_0728(const uint8_t* pkt, size_t size);
        bool decodeMsopPkt_0728Reflectivity(const uint8_t* pkt, size_t size);

#ifdef ENABLE_TRANSFORM
        bool decodeMsopPkt_1122(const uint8_t* pkt, size_t size);
        bool decodeMsopPkt_1122Reflectivity(const uint8_t* pkt, size_t size);
#endif 

    };

    template <typename T_PointCloud>
    inline WJDecoderMechConstParam& DecoderVanjee740<T_PointCloud>::getConstParam(uint8_t mode)
    {
        switch (mode)
        {
        case 1:
        {
            static WJDecoderMechConstParam param =
                {
                    1324 
                    ,48 
                    ,4 
                    ,144 
                    ,0.5f 
                    ,210.0f 
                    ,0.004f 
                    ,80.0f 
                };
            param.BLOCK_DURATION = 0.1 / 360;
            return param;
        }
        break;

        case 2:
        {
            static WJDecoderMechConstParam param =
                {
                    1064 
                    ,48 
                    ,4 
                    ,48 
                    ,0.2f 
                    ,200.0f 
                    ,0.004f 
                    ,80.0f 
                };
            param.BLOCK_DURATION = 0.1 / 360;
            return param;
        }
        break;
        }
        static WJDecoderMechConstParam param =
                {
                    1324 
                    ,48 
                    ,4 
                    ,144 
                    ,0.2f 
                    ,200.0f 
                    ,0.004f 
                    ,80.0f 
                };
            param.BLOCK_DURATION = 0.1 / 360;
            return param;
    }
    
    template <typename T_PointCloud> 
    inline WJEchoMode DecoderVanjee740<T_PointCloud>::getEchoMode(uint8_t mode)
    {
        switch (mode)
        {
        case 0x10:    
            return WJEchoMode::ECHO_DUAL;
        case 0x20:
        case 0x30:
        
        default:
            return WJEchoMode::ECHO_SINGLE;
        }
    }
    
    template <typename T_PointCloud>
    inline DecoderVanjee740<T_PointCloud>::DecoderVanjee740(const WJDecoderParam &param)
    : DecoderMech<T_PointCloud>(getConstParam(param.publish_mode), param)
    {
        if(param.max_distance < param.min_distance)
            WJ_WARNING << "config params (max distance < min distance)!" << WJ_REND;
            
        this->packet_duration_ = FRAME_DURATION / SINGLE_PKT_NUM;
        split_strategy_ = std::make_shared<SplitStrategyByAngle>(0);

        if(this->param_.config_from_file)
        {
            int ret_angle = this->chan_angles_.loadFromFile(this->param_.angle_path_ver, this->param_.angle_path_hor);
        }

        // WJ_INFOL << "740AlgorithmEnb: " << algorithm_enable_ << WJ_REND;
        // WJ_INFOL << "blank_point_compen_enable_: " << blank_point_compen_enable_ << WJ_REND;
        // WJ_INFOL << "outlier_enable_: " << outlier_enable_ << WJ_REND;
        // WJ_INFOL << "close_range_outliers_enable_: " << close_range_outliers_enable_ << WJ_REND;
        
        Filterate::GetInstance()->SetAlgorithmEnb(algorithm_enable_);
        Filterate::GetInstance()->SetBlankPointCompenEnb(blank_point_compen_enable_);
        Filterate::GetInstance()->SetOutlierEnb(outlier_enable_);
        Filterate::GetInstance()->SetCloseRangeOutliersEnb(close_range_outliers_enable_);
        
        m_qmap_one_circle_point_->Init(144);
        m_qmap_one_circle_point_->Clear();
        m_qmap_pcap_one_circle_point_->Init(144);
        m_qmap_pcap_one_circle_point_->Clear();
    }

    template <typename T_PointCloud>
    inline uint32 DecoderVanjee740<T_PointCloud>::GetOnePointInfoKey(const OnePointInfo& val)
    {
        if(val.echo_type == 1)
        {
            return ((val.line_id - 1) * 3600) + val.h_angle;
        }
        else if(val.echo_type == 2)
        {
            return ((val.line_id - 1) * 3600) + val.h_angle + ONE_CIRCLE_CLOUDS;
        }
        else
        {
            //qDebug() << "异常";
            return 0;
        }
    }
    
#ifdef ENABLE_TRANSFORM
    template <typename T_PointCloud>
    inline void DecoderVanjee740<T_PointCloud>::pointsCompense(OnePointInfoManage &points_info, const Eigen::Quaterniond q, const Eigen::Vector3d pos)
    {
        if(points_info.index_record_size_ < 1)
            return;
        //需求：点云xyz，各点线号，原排序不变
        Eigen::MatrixXd cloud_mirri_0(3, points_info.index_record_size_);
        // Eigen::MatrixXd cloud_mirri_1(3, points_info->index_record_size_);
        Eigen::MatrixXd cloud_mirri_2(3, points_info.index_record_size_);

        Eigen::MatrixXd pos_mirri_0(3, points_info.index_record_size_);
        // Eigen::MatrixXd pos_mirri_1(3, points_info->index_record_size_);
        Eigen::MatrixXd pos_mirri_2(3, points_info.index_record_size_);

        int cloud_size_mirro_0 = 0;
        // int cloud_size_mirro_1 = 0;
        int cloud_size_mirro_2 = 0;

        std::vector<int> index_mirro_0;
        // std::vector<int> index_mirro_1;
        std::vector<int> index_mirro_2;

        for(unsigned long i = 0; i < points_info.index_record_size_; i++)
        {
            OnePointInfo& point_info = points_info.GetDataIt(i);
            if(point_info.rotating_mirror_id == 0)    //第1面镜子
            {
                cloud_mirri_0(0, cloud_size_mirro_0) = point_info.x;
                cloud_mirri_0(1, cloud_size_mirro_0) = point_info.y;
                cloud_mirri_0(2, cloud_size_mirro_0) = point_info.z;

                pos_mirri_0(0, cloud_size_mirro_0) = pos(0);
                pos_mirri_0(1, cloud_size_mirro_0) = pos(1);
                pos_mirri_0(2, cloud_size_mirro_0) = pos(2);

                cloud_size_mirro_0 ++;
                index_mirro_0.push_back(i);
            }
            // else if(point_info.rotating_mirror_id == 1) //第2面镜子
            // {
            //     cloud_mirri_1(0, cloud_size_mirro_1) = point_info.x;
            //     cloud_mirri_1(1, cloud_size_mirro_1) = point_info.y;
            //     cloud_mirri_1(2, cloud_size_mirro_1) = point_info.z;

            //     pos_mirri_1(0, cloud_size_mirro_1) = pos(0);
            //     pos_mirri_1(1, cloud_size_mirro_1) = pos(1);
            //     pos_mirri_1(2, cloud_size_mirro_1) = pos(2);

            //     cloud_size_mirro_1 ++;
            //     index_mirro_1.push_back(i);
            // }
            else if(point_info.rotating_mirror_id == 2) //第3面镜子
            {
                cloud_mirri_2(0, cloud_size_mirro_2) = point_info.x;
                cloud_mirri_2(1, cloud_size_mirro_2) = point_info.y;
                cloud_mirri_2(2, cloud_size_mirro_2) = point_info.z;

                pos_mirri_2(0, cloud_size_mirro_2) = pos(0);
                pos_mirri_2(1, cloud_size_mirro_2) = pos(1);
                pos_mirri_2(2, cloud_size_mirro_2) = pos(2);

                cloud_size_mirro_2 ++;
                index_mirro_2.push_back(i);
            }
        }

        cloud_mirri_0.conservativeResize(3, cloud_size_mirro_0);
        // cloud_mirri_1.conservativeResize(3, cloud_size_mirro_1);
        cloud_mirri_2.conservativeResize(3, cloud_size_mirro_2);

        pos_mirri_0.conservativeResize(3, cloud_size_mirro_0);
        // pos_mirri_1.conservativeResize(3, cloud_size_mirro_1);
        pos_mirri_2.conservativeResize(3, cloud_size_mirro_2);

        //Eigen::MatrixXd cloud_mirri_0(3, points_info.index_record_size_);
        Eigen::MatrixXd cloud_compensate_0 = q.conjugate().matrix() * cloud_mirri_0 - pos_mirri_0;
        Eigen::MatrixXd cloud_compensate_2 = q.matrix() * cloud_mirri_2 + pos_mirri_2;

        // auto cloud_compensate_0 = q.conjugate() * cloud_mirri_0 - pos_mirri_0;
        // // auto cloud_compensate_1 = q.conjugate() * cloud_mirri_1 + pos_mirri_1;
        // auto cloud_compensate_2 = q * cloud_mirri_2 + pos_mirri_2;

        for(unsigned long i = 0; i < index_mirro_0.size(); i++)
        {
            points_info.ResetPointValue(index_mirro_0[i], cloud_compensate_0(0, i), cloud_compensate_0(1, i), cloud_compensate_0(2, i));
        }

        // for(int i = 0; i < index_mirro_1.size(); i++)
        // {
        //     points_info.ResetPointValue(index_mirro_1[i], cloud_compensate_1(0, i), cloud_compensate_1(1, i), cloud_compensate_1(2, i));
        // }

        for(unsigned long i = 0; i < index_mirro_2.size(); i++)
        {
            points_info.ResetPointValue(index_mirro_2[i], cloud_compensate_2(0, i), cloud_compensate_2(1, i), cloud_compensate_2(2, i));
        }
    }

    template <typename T_PointCloud>
    inline void DecoderVanjee740<T_PointCloud>::pointCompense(OnePointInfo* point_info, const Eigen::Quaterniond q, const Eigen::Vector3d pos)
    {
        Eigen::Vector3d point(point_info->x, point_info->y, point_info->z);
        if(point_info->rotating_mirror_id == 0)
        {
            Eigen::Vector3d p_compensate = q.conjugate() * point - pos;
            point_info->x = p_compensate(0);
            point_info->y = p_compensate(1);
            //point_info->z = p_compensate(2);  
        }
        else if(point_info->rotating_mirror_id == 2)
        {
            Eigen::Vector3d p_compensate = q * point + pos;
            point_info->x = p_compensate(0);
            point_info->y = p_compensate(1);
            //point_info->z = p_compensate(2);  
        }
    }

    template <typename T_PointCloud>
    inline void DecoderVanjee740<T_PointCloud>::pkgPointsCompense(OnePointInfoManage &points_info, const Eigen::Quaterniond q, const Eigen::Vector3d pos, int block_num)
    {
        unsigned int pkg_point_num = 48 * block_num;
        Eigen::MatrixXd pos_t(3, 1);    
        pos_t(0, 0) = pos(0);
        pos_t(1, 0) = pos(1);
        pos_t(2, 0) = pos(2);

        //需求：点云xyz，各点线号，原排序不变
        Eigen::MatrixXd cloud_mirri_0(3, pkg_point_num);
        Eigen::MatrixXd cloud_mirri_2(3, pkg_point_num);

        int cloud_size_mirro_0 = 0;
        int cloud_size_mirro_2 = 0;

        std::vector<int> index_mirro_0;
        std::vector<int> index_mirro_2;
        
        if(points_info.index_record_size_ < pkg_point_num)
            pkg_point_num = points_info.index_record_size_;

        for(int i = 0; i < pkg_point_num; i++)
        {
            OnePointInfo& point_info = points_info.GetDataIt(points_info.index_record_size_ - pkg_point_num + i);
            if(point_info.rotating_mirror_id == 0)    //第1面镜子
            {
                cloud_mirri_0(0, cloud_size_mirro_0) = point_info.x;
                cloud_mirri_0(1, cloud_size_mirro_0) = point_info.y;
                cloud_mirri_0(2, cloud_size_mirro_0) = point_info.z;

                cloud_size_mirro_0 ++;
                index_mirro_0.push_back(i);
            }
            else if(point_info.rotating_mirror_id == 2) //第3面镜子
            {
                cloud_mirri_2(0, cloud_size_mirro_2) = point_info.x;
                cloud_mirri_2(1, cloud_size_mirro_2) = point_info.y;
                cloud_mirri_2(2, cloud_size_mirro_2) = point_info.z;

                cloud_size_mirro_2 ++;
                index_mirro_2.push_back(i);
            }
        }

        cloud_mirri_0.conservativeResize(3, cloud_size_mirro_0);
        cloud_mirri_2.conservativeResize(3, cloud_size_mirro_2);

        Eigen::MatrixXd pos_mirri_0(3, cloud_size_mirro_0);
        Eigen::MatrixXd pos_mirri_2(3, cloud_size_mirro_2);

        pos_mirri_0 = pos_t.replicate(1,cloud_size_mirro_0);
        pos_mirri_2 = pos_t.replicate(1,cloud_size_mirro_2);

        Eigen::MatrixXd cloud_compensate_0 = q.conjugate().matrix() * cloud_mirri_0 - pos_mirri_0;
        Eigen::MatrixXd cloud_compensate_2 = q.matrix() * cloud_mirri_2 + pos_mirri_2;

        for(unsigned long i = 0; i < index_mirro_0.size(); i++)
        {
            points_info.ResetPointValue(points_info.index_record_size_ - pkg_point_num + index_mirro_0[i], cloud_compensate_0(0, i), cloud_compensate_0(1, i), cloud_compensate_0(2, i));
        }

        for(unsigned long i = 0; i < index_mirro_2.size(); i++)
        {
            points_info.ResetPointValue(points_info.index_record_size_ - pkg_point_num + index_mirro_2[i], cloud_compensate_2(0, i), cloud_compensate_2(1, i), cloud_compensate_2(2, i));
        }
    }
#endif

    template <typename T_PointCloud>
    inline bool DecoderVanjee740<T_PointCloud>::decodeMsopPkt(const uint8_t* pkt, size_t size)
    {
        static const int ScanData0728Size = sizeof(Params_ScanData0728::ScanData);
        static const int ScanData0728ReflectivitySize = sizeof(Params_ScanData0728Reflectivity::ScanData);
        static const int ScanData1122Size = sizeof(Params_ScanData1122::ScanData);
        static const int ScanData1122ReflectivitySize = sizeof(Params_ScanData1122Reflectivity::ScanData);
        switch(pkt[6])
        {
            case 0x02:
            {
                switch(size)
                {
                    case ScanData0728Size:
                    {
                        return decodeMsopPkt_0728(pkt , size);
                    }
                    break;
#ifdef ENABLE_TRANSFORM
                    case ScanData1122Size:
                    {
                        return decodeMsopPkt_1122(pkt , size);
                    }
                    break;
#endif
                }         
            }
            break;

            case 0x16:
            {
                switch(size)
                {
                    case ScanData0728ReflectivitySize:
                    {
                        return decodeMsopPkt_0728Reflectivity(pkt , size);
                    }
                    break;
#ifdef ENABLE_TRANSFORM
                    case ScanData1122ReflectivitySize:
                    {
                        return decodeMsopPkt_1122Reflectivity(pkt , size);
                    }
                    break;
#endif
                }
            }
            break;

        }
        return false;
    }

    template <typename T_PointCloud>
    inline bool DecoderVanjee740<T_PointCloud>::decodeMsopPkt_0728(const uint8_t* pkt, size_t size)
    {
        bool ret = false; 
        double pkt_ts = 0;
        static int pre_frame_id = 0;
        int one_circle_data_amount = 0;
        static int cur_circle_frame_num_publish = 0;
        static bool compulsion_publish = false;
        static int pre_angle = 0;
        static int pre_resolution_ratio = 0;

        const Vanjee740MsopPkt0728& packet = *((Vanjee740MsopPkt0728*)pkt);

        int32_t loss_packets_num = (packet.difop.frame_index + 65536 - pre_frame_id_) % 65536;
        if(loss_packets_num > 100 && pre_frame_id_ >= 0)
            WJ_WARNING << "loss " << loss_packets_num << " packets" << WJ_REND;
        pre_frame_id_ = packet.difop.frame_index;

        if(!this->param_.use_lidar_clock)
            pkt_ts = getTimeHost() * 1e-6;
        else
            pkt_ts = packet.difop.time_field.ntp_time.second + (packet.difop.time_field.ntp_time.microsecond * 1e-6);
            
        if(packet.data_block[0].echo_type != packet.data_block[1].echo_type)
        {
            //one_circle_data_amount = (1200 * 3) / 4;
            one_circle_data_amount = 900;
        }
        else
        {
            //one_circle_data_amount = (1200 * 3) / 8;
            one_circle_data_amount = 450;
        }

        unsigned short packet_num_diff = packet.difop.frame_index - pre_frame_id; // 和上一帧序号差

        if(packet_num_diff == 1)
        {
            cur_circle_frame_num_publish++;
        }
        else
        {
            cur_circle_frame_num_publish += (packet_num_diff - 1);
        }

        if(cur_circle_frame_num_publish > one_circle_data_amount)
        {
            compulsion_publish = true;
        }
        pre_frame_id = packet.difop.frame_index;

        for(int i = 0; i < 8;i++)
        {
            // 转镜号
            unsigned char rotating_mirror = packet.data_block[i].rotate_mirror_id;
            // 一个数据块 1个水平角度
            short h_angle = packet.data_block[i].horizontal_angle - 1; // 精度0.1
            // 一个数据块 1个垂直方向补偿角
            int v_compensate_angle = packet.data_block[i].vertical_angle_compensation;
            // 一个数据块 1个水平方向补偿角
            int h_compensate_angle = packet.data_block[i].horizontal_angle_compensation;
            // 获取水平角度分辨率
            unsigned char resolution_ratio = packet.data_block[i].horizontal_angle_res;
            // 获取回波类型
            unsigned char echo_type = packet.data_block[i].echo_type;

            if(packet.data_block[0].echo_type != packet.data_block[1].echo_type &&
                Decoder<T_PointCloud>::param_.publish_mode != 2 &&
                Decoder<T_PointCloud>::param_.publish_mode+1 != echo_type)
              continue;
           
            // 跳过异常数据
            if((rotating_mirror > 3) || (h_angle > 3600) || (resolution_ratio <=0) || (resolution_ratio > 2))
            {
                continue;
            }

            if(pre_resolution_ratio != resolution_ratio)
            {
                Filterate::GetInstance()->SetHResolution(resolution_ratio);
                if(resolution_ratio == 1)
                {
                    rotation_rate_ = 600; // 600转 / 分
                    h_angle_1_6us_ = ((360.0 / ((1000.0 / (rotation_rate_ / 60.0)) * 1000.0)) * 1.6 * 2) * 1000;
                     
                     if(Filterate::GetInstance()->GetStartAngle() != 1)
                    {
                        Filterate::GetInstance()->SetStartAngle(1);
                    }
                }
                else
                {
                    rotation_rate_ = 1200; // 1200转 / 分
                    h_angle_1_6us_ = ((360.0 / ((1000.0 / (rotation_rate_ / 60.0)) * 1000.0)) * 1.6 * 2) * 1000;
                
                    if(h_angle % 2 == 1)
                    {
                        if(Filterate::GetInstance()->GetStartAngle() != 1)
                        {
                            Filterate::GetInstance()->SetStartAngle(1);
                        }
                    }
                    else
                    {
                        if(Filterate::GetInstance()->GetStartAngle() != 0)
                        {
                            Filterate::GetInstance()->SetStartAngle(0);
                        }
                    }
                }               
            }

            if((h_angle < pre_angle && rotating_mirror == 0) || compulsion_publish)
            {

                if(m_qmap_one_circle_point_->index_record_size_ < 4000)
                {
                    m_qmap_one_circle_point_->Clear();
                }

                if(algorithm_enable_)
                {
                    // 发布前 走补点算法
                    int mode = 1;
                    if(packet.data_block[0].echo_type != packet.data_block[1].echo_type)
                    {
                      mode  = Decoder<T_PointCloud>::param_.publish_mode + 1;
                    }
                    else
                    {
                      mode = packet.data_block[0].echo_type;
                    }
                    Filterate::GetInstance()->filter(*m_qmap_one_circle_point_, *m_qmap_pcap_one_circle_point_,mode);
                }

                OnePointInfoManage* tmp  = nullptr;
                tmp = m_qmap_pcap_one_circle_point_;
                m_qmap_pcap_one_circle_point_ = m_qmap_one_circle_point_;
                m_qmap_one_circle_point_ = tmp;
                tmp = nullptr;

                // 将m_qMapOneCirclePoint数据转为ros发布结构体
                for(int i = 0; i < m_qmap_one_circle_point_->index_record_size_;i++)
                {
                    OnePointInfo& tem = m_qmap_one_circle_point_->GetDataIt(i);

                    if(outlier_enable_)
                    {
                        if(tem.non_filterable == 0 && (tem.distance > Filterate::GetInstance()->GetFullSearchScope()))
                        {
                             tem.x = 0;
                             tem.y = 0;
                             tem.z = 0;
                             tem.distance = 0;
                            //continue; // 不知道为啥 这个程序架构不允许 continue 否则发布点云的时候会报错
                        }
                            
                    }
                    if(close_range_outliers_enable_)
                    {
                        if(tem.close_range_outliers == 0 && (tem.distance < Filterate::GetInstance()->GetFullSearchScope()))
                        {
                             tem.x = 0;
                             tem.y = 0;
                             tem.z = 0;
                             tem.distance = 0;
                            //continue;
                        }
                    }                    

                    typename T_PointCloud::PointT point;
                    setX(point, tem.x);
                    setY(point, tem.y);
                    setZ(point, tem.z);

                    // // 如果开启了算法 可能会失效
                    // if((MaxIntensityVal != 0) && (MaxIntensityVal == tem.intensity) && (MaxIntensityFlag == 0) /*&& tem.distance > 0*/)
                    // {
                    //     tem.intensity = 255;
                    //     MaxIntensityFlag = 1;
                    // }
                    // // 如果开启了算法 可能会失效
                    // if((MinIntensityVal != 0) && (MinIntensityVal == tem.intensity) && (MinIntensityFlag == 0) /*&& tem.distance > 0*/ )
                    // {
                    //     tem.intensity = 0;
                    //     MinIntensityFlag = 1;
                    // }
                    

                    setIntensity(point, tem.intensity);
                    //setTimestamp(point, this->prev_pkt_ts_);
                    setTimestamp(point, tem.time / 1000.0);
                    setRing(point, tem.line_id);
                   
                    this->point_cloud_->points.emplace_back(std::move(point));
                }

                compulsion_publish = false;
                cur_circle_frame_num_publish = 0;
                this->cb_split_frame_(144, this->cloudTs());
                this->first_point_ts_ = pkt_ts;

                m_qmap_one_circle_point_->Clear();
                ret = true;
            }
            pre_angle = h_angle;

            for(int j = 0; j < 48;j++)
            {
                int l_line = j+1;
                int l_line_id;
                int h_angle_temp = h_angle; // 由于在for循环中 17-32线需要+38 因此增加临时变量 获取当前水平角度
                int zero_h_angle_temp = h_angle; // 从0开始算的水平角度
                float distance = packet.data_block[i].channel_data[j].distance << 2;
                distance/=1000;

                // int l_i = j % 8;
                // int l_j = j / 8;
                // int strong_weak =  (packet.Data.dataBlock[i].strong_weak[l_j] & (0x01 << l_i)) > 0?1:2;

                // if(strong_weak == 1)
                // {
                //     // 4.0米内禁止出强光
                //     if(distance <= 4.0) 
                //     {
                //         distance = 0;
                //     }
                // }

                if(l_line >16 && l_line <= 32)
                {
                    h_angle_temp += 38; // 3.8度的偏移
                }
                // 水平发光时序补偿
                int h_lum_timing_comp =  (((h_angle_lum_comp_[j] - 1) * h_angle_1_6us_));
                double points_time = pkt_ts + ((h_angle_lum_comp_[j] - 1) * 1.6) * 1e-6 ;

                h_angle_temp = (int)(((h_angle_temp*100 + h_compensate_angle ) + h_lum_timing_comp )+ 360000)%360000;//获取水平角 精度0.01


                switch(rotating_mirror)
                {
                case 0:
                    l_line_id = (l_line - 1)*3+rotating_mirror + 2;
                    break;
                case 1:
                    l_line_id = (l_line - 1)*3+rotating_mirror + 0;
                    break;
                case 2:
                    l_line_id = (l_line - 1)*3+rotating_mirror + 1;
                    break;
                }

                int l_iomdex = this->chan_angles_.vertAdjust(j);
                l_iomdex = this->chan_angles_.horiz_vertAdjust(l_line_id - 1, h_angle);
                l_iomdex = (int)(l_iomdex + 360000) % 360000;

                int32_t azimuth_index = h_angle_temp;
                int32_t verticalVal_740 = l_iomdex;

                float xy,x,y,z;


                xy = distance * COS(verticalVal_740);
                x = xy * (COS(azimuth_index));
                y = xy * SIN(azimuth_index);
                z = distance * SIN(verticalVal_740);
                this->transformPoint(x, y, z);

                OnePointInfo one_point_info_tmp;
                one_point_info_tmp.time = points_time;
                one_point_info_tmp.rotating_mirror_id = rotating_mirror;
                one_point_info_tmp.ld_id = l_line;
                // 重新映射ros反射率颜色 将 255 -> 0 , 0 -> 255  ,255 的最大值修改至 0.75 目的是 跳过ros紫色区域
                // 255 - 0 颜色依次为 深蓝-浅蓝-绿色-黄色-红色-紫色 -因此将255再次映射到145 防止出现紫色
                one_point_info_tmp.intensity = (255-packet.data_block[i].channel_data[j].intensity) * 0.75;
                one_point_info_tmp.low_thres_pulsewidth = 0;
                one_point_info_tmp.high_thres_pulsewidth = 0;
                one_point_info_tmp.old_distance = distance * 1000;
                one_point_info_tmp.distance = distance * 1000;
                one_point_info_tmp.azimuth = h_angle_temp / 10; // 精度0.01
                one_point_info_tmp.vazimuth = l_iomdex;
                one_point_info_tmp.h_angle = packet.data_block[i].horizontal_angle;
                one_point_info_tmp.h_angle = (int)(one_point_info_tmp.h_angle + 3600)%3600;//获取水平角 精度0.01
                one_point_info_tmp.v_angle = this->chan_angles_.horiz_vertAdjust(l_line_id - 1, h_angle);//Cfg.opticalPathAngle_Line[l_line_id - 1].v_angle;
                one_point_info_tmp.echo_type = echo_type; // 1- 最强 2-最后 // 主要用于显示在表中
                one_point_info_tmp.strong_weak = 0;
                one_point_info_tmp.y = y;//新坐标系计算方式，逆时针改顺时针的方式
                one_point_info_tmp.x = x;
                one_point_info_tmp.z = z;//不补偿
                one_point_info_tmp.line_id = l_line_id;//3*(l_line-1)+rotating_mirror+1;

                one_point_info_tmp.index = GetOnePointInfoKey(one_point_info_tmp);
                m_qmap_one_circle_point_->insert(one_point_info_tmp.index,std::move(one_point_info_tmp));
                
            }
        }       
        return ret;        
    }

    template <typename T_PointCloud>
    inline bool DecoderVanjee740<T_PointCloud>::decodeMsopPkt_0728Reflectivity(const uint8_t* pkt, size_t size)
    {
        bool ret = false; 
        double pkt_ts = 0;
        static int pre_frame_id = 0;
        int one_circle_data_amount = 0;
        static int cur_circle_frame_num_publish = 0;
        static bool compulsion_publish = false;
        static int pre_angle = 0;
        static int pre_resolution_ratio = 0;

        const Vanjee740MsopPkt0728WithPulseWidth& packet = *(Vanjee740MsopPkt0728WithPulseWidth*)pkt;

        int32_t loss_packets_num = (packet.difop.frame_index + 65536 - pre_frame_id_) % 65536;
        if(loss_packets_num > 100 && pre_frame_id_ >= 0)
            WJ_WARNING << "loss " << loss_packets_num << " packets" << WJ_REND;
        pre_frame_id_ = packet.difop.frame_index;

        if(!this->param_.use_lidar_clock)
            pkt_ts = getTimeHost() * 1e-6;
        else
            pkt_ts = packet.difop.time_field.ntp_time.second + (packet.difop.time_field.ntp_time.microsecond * 1e-6);

        if(packet.data_block[0].echo_type != packet.data_block[1].echo_type)
        {
            //one_circle_data_amount = (1200 * 3) / 2;
            one_circle_data_amount = 1800;
        }
        else
        {
            //one_circle_data_amount = (1200 * 3) / 4;
            one_circle_data_amount = 900;
        }

        unsigned short packet_num_diff = packet.difop.frame_index - pre_frame_id; // 和上一帧序号差
        if(packet_num_diff == 1)
        {
            cur_circle_frame_num_publish++;
        }
        else
        {
            cur_circle_frame_num_publish += (packet_num_diff - 1);
        }

        if(cur_circle_frame_num_publish > one_circle_data_amount)
        {
            compulsion_publish = true;
        }
        pre_frame_id = packet.difop.frame_index;

        for(int i = 0; i < 4;i++)
        {
            // 转镜号
            unsigned char rotating_mirror = packet.data_block[i].rotate_mirror_id;
            // 一个数据块 1个水平角度
            short h_angle = packet.data_block[i].horizontal_angle - 1; // 精度0.1
            // 一个数据块 1个垂直方向补偿角
            int v_compensate_angle = packet.data_block[i].vertical_angle_compensation;
            // 一个数据块 1个水平方向补偿角
            int h_compensate_angle = packet.data_block[i].horizontal_angle_compensation;
            // 获取水平角度分辨率
            unsigned char resolution_ratio = packet.data_block[i].horizontal_angle_res;
            // 获取回波类型
            unsigned char echo_type = packet.data_block[i].echo_type;

            if(packet.data_block[0].echo_type != packet.data_block[1].echo_type &&
                Decoder<T_PointCloud>::param_.publish_mode != 2 &&
                Decoder<T_PointCloud>::param_.publish_mode+1 != echo_type)
              continue;
            
            // 跳过异常数据
            if((rotating_mirror > 3) || (h_angle > 3600) || (resolution_ratio <=0) || (resolution_ratio > 2))
            {
                continue;
            }

            if(pre_resolution_ratio != resolution_ratio)
            {
                Filterate::GetInstance()->SetHResolution(resolution_ratio);
                if(resolution_ratio == 1)
                {
                    rotation_rate_ = 600; // 600转 / 分
                    h_angle_1_6us_ = ((360.0 / ((1000.0 / (rotation_rate_ / 60.0)) * 1000.0)) * 1.6 * 2) * 1000;
                     
                     if(Filterate::GetInstance()->GetStartAngle() != 1)
                    {
                        Filterate::GetInstance()->SetStartAngle(1);
                    }
                }
                else
                {
                    rotation_rate_ = 1200; // 1200转 / 分
                    h_angle_1_6us_ = ((360.0 / ((1000.0 / (rotation_rate_ / 60.0)) * 1000.0)) * 1.6 * 2) * 1000;
                
                    if(h_angle % 2 == 1)
                    {
                        if(Filterate::GetInstance()->GetStartAngle() != 1)
                        {
                            Filterate::GetInstance()->SetStartAngle(1);
                        }
                    }
                    else
                    {
                        if(Filterate::GetInstance()->GetStartAngle() != 0)
                        {
                            Filterate::GetInstance()->SetStartAngle(0);
                        }
                    }
                }               
            }

            if((h_angle < pre_angle && rotating_mirror == 0) || compulsion_publish)
            {

                if(m_qmap_one_circle_point_->index_record_size_ < 4000)
                {
                    m_qmap_one_circle_point_->Clear();
                }

                if(algorithm_enable_)
                {
                    // 发布前 走补点算法
                    int mode = 1;
                    if(packet.data_block[0].echo_type != packet.data_block[1].echo_type)
                    {
                      mode  = Decoder<T_PointCloud>::param_.publish_mode + 1;
                    }
                    else
                    {
                      mode = packet.data_block[0].echo_type;
                    }
                    Filterate::GetInstance()->filter(*m_qmap_one_circle_point_, *m_qmap_pcap_one_circle_point_,mode);
                }

                OnePointInfoManage* tmp = nullptr;
                tmp = m_qmap_pcap_one_circle_point_;
                m_qmap_pcap_one_circle_point_ = m_qmap_one_circle_point_;
                m_qmap_one_circle_point_ = tmp;
                tmp = nullptr;

                // int MaxIntensityFlag = 0;
                // int MinIntensityFlag = 0;

                // for(int i = 0; i < m_qmap_one_circle_point_.index_record_size_;i++)
                // {
                //     OnePointInfo& tem = m_qmap_one_circle_point_.GetDataIt(i);
                //     if(MaxIntensityVal < tem.intensity /*&& tem.distance*/ > 0)
                //     {
                //         MaxIntensityVal = tem.intensity;
                //     }

                //     if(MinIntensityVal > tem.intensity /*&& tem.distance*/ > 0)
                //     {
                //         MinIntensityVal = tem.intensity;
                //     }
                // }

                // 将m_qMapOneCirclePoint数据转为ros发布结构体
                for(int i = 0; i < m_qmap_one_circle_point_->index_record_size_;i++)
                {
                    OnePointInfo& tem = m_qmap_one_circle_point_->GetDataIt(i);

                    if(outlier_enable_)
                    {
                        if(tem.non_filterable == 0 && (tem.distance > Filterate::GetInstance()->GetFullSearchScope()))
                        {
                             tem.x = 0;
                             tem.y = 0;
                             tem.z = 0;
                             tem.distance = 0;
                            //continue; // 不知道为啥 这个程序架构不允许 continue 否则发布点云的时候会报错
                        }                            
                    }
                    if(close_range_outliers_enable_)
                    {
                        if(tem.close_range_outliers == 0 && (tem.distance < Filterate::GetInstance()->GetFullSearchScope()))
                        {
                             tem.x = 0;
                             tem.y = 0;
                             tem.z = 0;
                             tem.distance = 0;
                            //continue;
                        }
                    }
                   
                    typename T_PointCloud::PointT point;
                    setX(point, tem.x);
                    setY(point, tem.y);
                    setZ(point, tem.z);

                    // // 如果开启了算法 可能会失效
                    // if((MaxIntensityVal != 0) && (MaxIntensityVal == tem.intensity) && (MaxIntensityFlag == 0) /*&& tem.distance > 0*/)
                    // {
                    //     tem.intensity = 255;
                    //     MaxIntensityFlag = 1;
                    // }
                    // // 如果开启了算法 可能会失效
                    // if((MinIntensityVal != 0) && (MinIntensityVal == tem.intensity) && (MinIntensityFlag == 0) /*&& tem.distance > 0*/ )
                    // {
                    //     tem.intensity = 0;
                    //     MinIntensityFlag = 1;
                    // }
                    
                    setIntensity(point, tem.intensity);
                    setTimestamp(point, tem.time);
                    setRing(point, tem.line_id);
                    
                    this->point_cloud_->points.emplace_back(std::move(point));
                    this->last_point_ts_ = tem.time;
                }

                compulsion_publish = false;
                cur_circle_frame_num_publish = 0;
                this->cb_split_frame_(144, this->cloudTs());
                this->first_point_ts_ = pkt_ts;

                m_qmap_one_circle_point_->Clear();
                ret = true;
            }
            pre_angle = h_angle;

            for(int j = 0; j < 48;j++)
            {
                int l_line = j+1;
                int l_line_id;
                int h_angle_temp = h_angle; // 由于在for循环中 17-32线需要+38 因此增加临时变量 获取当前水平角度
                int zero_h_angle_temp = h_angle; // 从0开始算的水平角度
                float distance = packet.data_block[i].channel_data[j].distance << 2;
                distance/=1000;

                int l_i = j % 8;
                int l_j = j / 8;
                int strong_weak =  (packet.data_block[i].strong_weak_flag[l_j] & (0x01 << l_i)) > 0?1:2;

                if(strong_weak == 1)
                {
                    // 4.0米内禁止出强光
                    if(distance <= 4.0) 
                    {
                        distance = 0;
                    }
                }

                if(l_line >16 && l_line <= 32)
                {
                    h_angle_temp += 38; // 3.8度的偏移
                }
                // 水平发光时序补偿
                int h_lum_timing_comp =  (((h_angle_lum_comp_[j] - 1) * h_angle_1_6us_));
                double points_time = pkt_ts + ((h_angle_lum_comp_[j] - 1) * 1.6) * 1e-6 ;

                h_angle_temp = (int)(((h_angle_temp*100 + h_compensate_angle ) + h_lum_timing_comp )+ 360000)%360000;//获取水平角 精度0.01

                switch(rotating_mirror)
                {
                case 0:
                    l_line_id = (l_line - 1)*3+rotating_mirror + 2;
                    break;
                case 1:
                    l_line_id = (l_line - 1)*3+rotating_mirror + 0;
                    break;
                case 2:
                    l_line_id = (l_line - 1)*3+rotating_mirror + 1;
                    break;
                }

                //int l_iomdex = this->chan_angles_.vertAdjust(j);
                int l_iomdex = this->chan_angles_.horiz_vertAdjust(l_line_id - 1, h_angle);
                l_iomdex = (int)(l_iomdex + 360000) % 360000;

                int32_t azimuth_index = h_angle_temp;
                int32_t verticalVal_740 = l_iomdex;
                //WJ_INFO << "l_line_id" << (l_line_id-1) << "    h_angle" << h_angle << "    vangle:" << verticalVal_740 << WJ_REND;
                float xy,x,y,z;


                xy = distance * COS(verticalVal_740);
                x = xy * (COS(azimuth_index));
                y = xy * SIN(azimuth_index);
                z = distance * SIN(verticalVal_740);
                this->transformPoint(x, y, z);

                OnePointInfo one_point_info_tmp;
                one_point_info_tmp.time = points_time;
                one_point_info_tmp.rotating_mirror_id = rotating_mirror;
                one_point_info_tmp.ld_id = l_line;
                // 重新映射ros反射率颜色 将 255 -> 0 , 0 -> 255  ,255 的最大值修改至 0.75 目的是 跳过ros紫色区域
                // 255 - 0 颜色依次为 深蓝-浅蓝-绿色-黄色-红色-紫色 -因此将255再次映射到145 防止出现紫色
                one_point_info_tmp.intensity = (255-packet.data_block[i].channel_data[j].intensity) * 0.75;
                one_point_info_tmp.low_thres_pulsewidth = packet.data_block[i].channel_data[j].low_thres_pulse_width;
                one_point_info_tmp.high_thres_pulsewidth = packet.data_block[i].channel_data[j].high_thres_pulse_width;
                one_point_info_tmp.old_distance = distance * 1000;
                one_point_info_tmp.distance = distance * 1000;
                one_point_info_tmp.azimuth = h_angle_temp / 10; // 精度0.01
                one_point_info_tmp.vazimuth = l_iomdex;
                one_point_info_tmp.h_angle = packet.data_block[i].horizontal_angle;
                one_point_info_tmp.h_angle = (int)(one_point_info_tmp.h_angle + 3600)%3600;//获取水平角 精度0.01
                one_point_info_tmp.v_angle = this->chan_angles_.horiz_vertAdjust(l_line_id - 1, h_angle);//Cfg.opticalPathAngle_Line[l_line_id - 1].v_angle;
                one_point_info_tmp.echo_type = echo_type; // 1- 最强 2-最后 // 主要用于显示在表中
                one_point_info_tmp.strong_weak = strong_weak;
                one_point_info_tmp.y = y;//新坐标系计算方式，逆时针改顺时针的方式
                one_point_info_tmp.x = x;
                one_point_info_tmp.z = z;//不补偿
                one_point_info_tmp.line_id = l_line_id;//3*(l_line-1)+rotating_mirror+1;

                one_point_info_tmp.index = GetOnePointInfoKey(one_point_info_tmp);
                m_qmap_one_circle_point_->insert(one_point_info_tmp.index,std::move(one_point_info_tmp));               
            }
        }       
        return ret;       
    }

#ifdef ENABLE_TRANSFORM
    template <typename T_PointCloud>
    inline bool DecoderVanjee740<T_PointCloud>::decodeMsopPkt_1122(const uint8_t* pkt, size_t size)
    {
        bool ret = false; 
        double pkt_ts = 0;
        static int pre_frame_id = 0;
        int one_circle_data_amount = 0;
        static int cur_circle_frame_num_publish = 0;
        static bool compulsion_publish = false;
        static int pre_angle = 0;
        static int pre_resolution_ratio = 0;

        const Vanjee740MsopPkt1122& packet = *((Vanjee740MsopPkt1122*)pkt);

        int32_t loss_packets_num = (packet.difop.frame_index + 65536 - pre_frame_id_) % 65536;
        if(loss_packets_num > 100 && pre_frame_id_ >= 0)
            WJ_WARNING << "loss " << loss_packets_num << " packets" << WJ_REND;
        pre_frame_id_ = packet.difop.frame_index;

        if(!this->param_.use_lidar_clock)
            pkt_ts = getTimeHost() * 1e-6;
        else
            pkt_ts = packet.difop.time_field.ntp_time.second + (packet.difop.time_field.ntp_time.microsecond * 1e-6);

        if(packet.data_block[0].echo_type != packet.data_block[1].echo_type)
        {
            //one_circle_data_amount = (1200 * 3) / 4;
            one_circle_data_amount = 900;
        }
        else
        {
            //one_circle_data_amount = (1200 * 3) / 8;
            one_circle_data_amount = 450;
        }

        unsigned short packet_num_diff = packet.difop.frame_index - pre_frame_id; // 和上一帧序号差

        if(packet_num_diff == 1)
        {
            cur_circle_frame_num_publish++;
        }
        else
        {
            cur_circle_frame_num_publish += (packet_num_diff - 1);
        }

        if(cur_circle_frame_num_publish > one_circle_data_amount)
        {
            compulsion_publish = true;
        }
        pre_frame_id = packet.difop.frame_index;

#if POINTS_COMPENSE
        Eigen::Quaterniond quaternion(packet.data_imu.quaternion_w, packet.data_imu.quaternion_x, packet.data_imu.quaternion_y, packet.data_imu.quaternion_z);
        Eigen::Vector3d pos_cur(packet.data_imu.distance_offset_x * 0.004, packet.data_imu.distance_offset_y * 0.004, packet.data_imu.distance_offset_z * 0.004);
#endif 

        for(int i = 0; i < 8;i++)
        {
            // 转镜号
            unsigned char rotating_mirror = packet.data_block[i].rotate_mirror_id;
            // 一个数据块 1个水平角度
            short h_angle = packet.data_block[i].horizontal_angle - 1; // 精度0.1
            // 一个数据块 1个垂直方向补偿角
            int v_compensate_angle = packet.data_block[i].vertical_angle_compensation;
            // 一个数据块 1个水平方向补偿角
            int h_compensate_angle = packet.data_block[i].horizontal_angle_compensation;
            // 获取水平角度分辨率
            unsigned char resolution_ratio = packet.data_block[i].horizontal_angle_res;
            // 获取回波类型
            unsigned char echo_type = packet.data_block[i].echo_type;

            if(packet.data_block[0].echo_type != packet.data_block[1].echo_type &&
                Decoder<T_PointCloud>::param_.publish_mode != 2 &&
                Decoder<T_PointCloud>::param_.publish_mode+1 != echo_type)
              continue;
           
            // 跳过异常数据
            if((rotating_mirror > 3) || (h_angle > 3600) || (resolution_ratio <=0) || (resolution_ratio > 2))
            {
                continue;
            }

            if(pre_resolution_ratio != resolution_ratio)
            {
                Filterate::GetInstance()->SetHResolution(resolution_ratio);
                if(resolution_ratio == 1)
                {
                    rotation_rate_ = 600; // 600转 / 分
                    h_angle_1_6us_ = ((360.0 / ((1000.0 / (rotation_rate_ / 60.0)) * 1000.0)) * 1.6 * 2) * 1000;
                     
                     if(Filterate::GetInstance()->GetStartAngle() != 1)
                    {
                        Filterate::GetInstance()->SetStartAngle(1);
                    }
                }
                else
                {
                    rotation_rate_ = 1200; // 1200转 / 分
                    h_angle_1_6us_ = ((360.0 / ((1000.0 / (rotation_rate_ / 60.0)) * 1000.0)) * 1.6 * 2) * 1000;
                
                    if(h_angle % 2 == 1)
                    {
                        if(Filterate::GetInstance()->GetStartAngle() != 1)
                        {
                            Filterate::GetInstance()->SetStartAngle(1);
                        }
                    }
                    else
                    {
                        if(Filterate::GetInstance()->GetStartAngle() != 0)
                        {
                            Filterate::GetInstance()->SetStartAngle(0);
                        }
                    }
                }               
            }

            if((h_angle < pre_angle && rotating_mirror == 0) || compulsion_publish)
            // if(this->split_strategy_->newBlock(h_angle, 0) || compulsion_publish)
            {
                if(m_qmap_one_circle_point_->index_record_size_ < 4000)
                {
                    m_qmap_one_circle_point_->Clear();
                }

#if POINTS_COMPENSE && POINTCLOUD_COMPENSE && !SINGLE_POINT_COMPENSE && !PKG_POINT_COMPENSE
                pointsCompense(*m_qmap_one_circle_point_, quaternion, pos_cur);
#endif

                if(algorithm_enable_)
                {
                    // 发布前 走补点算法
                    int mode = 1;
                    if(packet.data_block[0].echo_type != packet.data_block[1].echo_type)
                    {
                      mode  = Decoder<T_PointCloud>::param_.publish_mode + 1;
                    }
                    else
                    {
                      mode = packet.data_block[0].echo_type;
                    }
                    Filterate::GetInstance()->filter(*m_qmap_one_circle_point_, *m_qmap_pcap_one_circle_point_,mode);
                }

                OnePointInfoManage* tmp  = nullptr;
                tmp = m_qmap_pcap_one_circle_point_;
                m_qmap_pcap_one_circle_point_ = m_qmap_one_circle_point_;
                m_qmap_one_circle_point_ = tmp;
                tmp = nullptr;

                // 将m_qMapOneCirclePoint数据转为ros发布结构体
                for(unsigned long i = 0; i < m_qmap_one_circle_point_->index_record_size_;i++)
                {
                    OnePointInfo& tem = m_qmap_one_circle_point_->GetDataIt(i);

                    if(outlier_enable_)
                    {
                        if(tem.non_filterable == 0 && (tem.distance > Filterate::GetInstance()->GetFullSearchScope()))
                        {
                             tem.x = 0;
                             tem.y = 0;
                             tem.z = 0;
                             tem.distance = 0;
                            //continue; // 不知道为啥 这个程序架构不允许 continue 否则发布点云的时候会报错
                        }
                            
                    }
                    if(close_range_outliers_enable_)
                    {
                        if(tem.close_range_outliers == 0 && (tem.distance < Filterate::GetInstance()->GetFullSearchScope()))
                        {
                             tem.x = 0;
                             tem.y = 0;
                             tem.z = 0;
                             tem.distance = 0;
                            //continue;
                        }
                    }

                    typename T_PointCloud::PointT point;
                    setX(point, tem.x);
                    setY(point, tem.y);
                    setZ(point, tem.z);

                    setIntensity(point, tem.intensity);
                    //setTimestamp(point, this->prev_pkt_ts_);
                    setTimestamp(point, tem.time / 1000.0);
                    setRing(point, tem.line_id);

                    this->point_cloud_->points.emplace_back(std::move(point));
                }

                compulsion_publish = false;
                cur_circle_frame_num_publish = 0;
                this->cb_split_frame_(144, this->cloudTs());
                this->first_point_ts_ = pkt_ts;
                
                m_qmap_one_circle_point_->Clear();
                ret = true;
            }
            pre_angle = h_angle;

            for(int j = 0; j < 48;j++)
            {
                int l_line = j+1;
                int l_line_id;
                int h_angle_temp = h_angle; // 由于在for循环中 17-32线需要+38 因此增加临时变量 获取当前水平角度
                int zero_h_angle_temp = h_angle; // 从0开始算的水平角度
                float distance = packet.data_block[i].channel_data[j].distance << 2;
                distance/=1000;

                if(l_line >16 && l_line <= 32)
                {
                    h_angle_temp += 38; // 3.8度的偏移
                }
                // 水平发光时序补偿
                int h_lum_timing_comp =  (((h_angle_lum_comp_[j] - 1) * h_angle_1_6us_));
                double points_time = pkt_ts + ((h_angle_lum_comp_[j] - 1) * 1.6) * 1e-6 ;

                h_angle_temp = (int)(((h_angle_temp*100 + h_compensate_angle ) + h_lum_timing_comp )+ 360000)%360000;//获取水平角 精度0.01


                switch(rotating_mirror)
                {
                case 0:
                    l_line_id = (l_line - 1)*3+rotating_mirror + 2;
                    break;
                case 1:
                    l_line_id = (l_line - 1)*3+rotating_mirror + 0;
                    break;
                case 2:
                    l_line_id = (l_line - 1)*3+rotating_mirror + 1;
                    break;
                }

                int l_iomdex = this->chan_angles_.vertAdjust(j);
                l_iomdex = this->chan_angles_.horiz_vertAdjust(l_line_id - 1, h_angle);
                l_iomdex = (int)(l_iomdex + 360000) % 360000;

                int32_t azimuth_index = h_angle_temp;
                int32_t verticalVal_740 = l_iomdex;

                float xy,x,y,z;


                xy = distance * COS(verticalVal_740);
                x = xy * (COS(azimuth_index));
                y = xy * SIN(azimuth_index);
                z = distance * SIN(verticalVal_740);
                this->transformPoint(x, y, z);

                OnePointInfo one_point_info_tmp;
                one_point_info_tmp.time = points_time;
                one_point_info_tmp.rotating_mirror_id = rotating_mirror;
                one_point_info_tmp.ld_id = l_line;
                // 重新映射ros反射率颜色 将 255 -> 0 , 0 -> 255  ,255 的最大值修改至 0.75 目的是 跳过ros紫色区域
                // 255 - 0 颜色依次为 深蓝-浅蓝-绿色-黄色-红色-紫色 -因此将255再次映射到145 防止出现紫色
                one_point_info_tmp.intensity = (255-packet.data_block[i].channel_data[j].intensity) * 0.75;
                one_point_info_tmp.low_thres_pulsewidth = 0;
                one_point_info_tmp.high_thres_pulsewidth = 0;
                one_point_info_tmp.old_distance = distance * 1000;
                one_point_info_tmp.distance = distance * 1000;
                one_point_info_tmp.azimuth = h_angle_temp / 10; // 精度0.01
                one_point_info_tmp.vazimuth = l_iomdex;
                one_point_info_tmp.h_angle = packet.data_block[i].horizontal_angle;
                one_point_info_tmp.h_angle = (int)(one_point_info_tmp.h_angle + 3600)%3600;//获取水平角 精度0.01
                one_point_info_tmp.v_angle = this->chan_angles_.horiz_vertAdjust(l_line_id - 1, h_angle);//Cfg.opticalPathAngle_Line[l_line_id - 1].v_angle;
                one_point_info_tmp.echo_type = echo_type; // 1- 最强 2-最后 // 主要用于显示在表中
                one_point_info_tmp.strong_weak = 0;
                one_point_info_tmp.y = y;//新坐标系计算方式，逆时针改顺时针的方式
                one_point_info_tmp.x = x;
                one_point_info_tmp.z = z;//不补偿
                one_point_info_tmp.line_id = l_line_id;//3*(l_line-1)+rotating_mirror+1;
#if POINTS_COMPENSE && !POINTCLOUD_COMPENSE && SINGLE_POINT_COMPENSE && !PKG_POINT_COMPENSE
                pointCompense(&one_point_info_tmp, quaternion, pos_cur);
#endif
                one_point_info_tmp.index = GetOnePointInfoKey(one_point_info_tmp);
                m_qmap_one_circle_point_->insert(one_point_info_tmp.index,std::move(one_point_info_tmp));
                
            }

        }     

#if POINTS_COMPENSE && !POINTCLOUD_COMPENSE && !SINGLE_POINT_COMPENSE && PKG_POINT_COMPENSE
        pkgPointsCompense(*m_qmap_one_circle_point_, quaternion, pos_cur, 8);
#endif  
        
        return ret;       
    }

    template <typename T_PointCloud>
    inline bool DecoderVanjee740<T_PointCloud>::decodeMsopPkt_1122Reflectivity(const uint8_t* pkt, size_t size)
    {
        bool ret = false; 
        double pkt_ts = 0;
        static int pre_frame_id = 0;
        int one_circle_data_amount = 0;
        static int cur_circle_frame_num_publish = 0;
        static bool compulsion_publish = false;
        static int pre_angle = 0;
        static int pre_resolution_ratio = 0;

        const Vanjee740MsopPkt1122WithPulseWidth& packet = *(Vanjee740MsopPkt1122WithPulseWidth*)pkt;

        int32_t loss_packets_num = (packet.difop.frame_index + 65536 - pre_frame_id_) % 65536;
        if(loss_packets_num > 100 && pre_frame_id_ >= 0)
            WJ_WARNING << "loss " << loss_packets_num << " packets" << WJ_REND;
        pre_frame_id_ = packet.difop.frame_index;

        if(!this->param_.use_lidar_clock)
            pkt_ts = getTimeHost() * 1e-6;
        else
            pkt_ts = packet.difop.time_field.ntp_time.second + (packet.difop.time_field.ntp_time.microsecond * 1e-6);

        if(packet.data_block[0].echo_type != packet.data_block[1].echo_type)
        {
            //one_circle_data_amount = (1200 * 3) / 2;
            one_circle_data_amount = 1800;
        }
        else
        {
            //one_circle_data_amount = (1200 * 3) / 4;
            one_circle_data_amount = 900;
        }

        unsigned short packet_num_diff = packet.difop.frame_index - pre_frame_id; // 和上一帧序号差
        if(packet_num_diff == 1)
        {
            cur_circle_frame_num_publish++;
        }
        else
        {
            cur_circle_frame_num_publish += (packet_num_diff - 1);
        }

        if(cur_circle_frame_num_publish > one_circle_data_amount)
        {
            compulsion_publish = true;
        }
        pre_frame_id = packet.difop.frame_index;

#if POINTS_COMPENSE
        Eigen::Quaterniond quaternion(packet.data_imu.quaternion_w, packet.data_imu.quaternion_x, packet.data_imu.quaternion_y, packet.data_imu.quaternion_z);
        Eigen::Vector3d pos_cur(packet.data_imu.distance_offset_x * 0.004, packet.data_imu.distance_offset_y * 0.004, packet.data_imu.distance_offset_z * 0.004);
#endif 

        for(int i = 0; i < 4;i++)
        {
            // 转镜号
            unsigned char rotating_mirror = packet.data_block[i].rotate_mirror_id;
            // 一个数据块 1个水平角度
            short h_angle = packet.data_block[i].horizontal_angle - 1; // 精度0.1
            // 一个数据块 1个垂直方向补偿角
            int v_compensate_angle = packet.data_block[i].vertical_angle_compensation;
            // 一个数据块 1个水平方向补偿角
            int h_compensate_angle = packet.data_block[i].horizontal_angle_compensation;
            // 获取水平角度分辨率
            unsigned char resolution_ratio = packet.data_block[i].horizontal_angle_res;
            // 获取回波类型
            unsigned char echo_type = packet.data_block[i].echo_type;

            if(packet.data_block[0].echo_type != packet.data_block[1].echo_type &&
                Decoder<T_PointCloud>::param_.publish_mode != 2 &&
                Decoder<T_PointCloud>::param_.publish_mode+1 != echo_type)
              continue;
            
            // 跳过异常数据
            if((rotating_mirror > 3) || (h_angle > 3600) || (resolution_ratio <=0) || (resolution_ratio > 2))
            {
                continue;
            }

            if(pre_resolution_ratio != resolution_ratio)
            {
                Filterate::GetInstance()->SetHResolution(resolution_ratio);
                if(resolution_ratio == 1)
                {
                    rotation_rate_ = 600; // 600转 / 分
                    h_angle_1_6us_ = ((360.0 / ((1000.0 / (rotation_rate_ / 60.0)) * 1000.0)) * 1.6 * 2) * 1000;
                     
                     if(Filterate::GetInstance()->GetStartAngle() != 1)
                    {
                        Filterate::GetInstance()->SetStartAngle(1);
                    }
                }
                else
                {
                    rotation_rate_ = 1200; // 1200转 / 分
                    h_angle_1_6us_ = ((360.0 / ((1000.0 / (rotation_rate_ / 60.0)) * 1000.0)) * 1.6 * 2) * 1000;
                
                    if(h_angle % 2 == 1)
                    {
                        if(Filterate::GetInstance()->GetStartAngle() != 1)
                        {
                            Filterate::GetInstance()->SetStartAngle(1);
                        }
                    }
                    else
                    {
                        if(Filterate::GetInstance()->GetStartAngle() != 0)
                        {
                            Filterate::GetInstance()->SetStartAngle(0);
                        }
                    }
                }               
            }

            if((h_angle < pre_angle && rotating_mirror == 0) || compulsion_publish)
            {

                if(m_qmap_one_circle_point_->index_record_size_ < 4000)
                {
                    m_qmap_one_circle_point_->Clear();
                }

#if POINTS_COMPENSE && POINTCLOUD_COMPENSE && !SINGLE_POINT_COMPENSE && !PKG_POINT_COMPENSE
                pointsCompense(*m_qmap_one_circle_point_, quaternion, pos_cur);
#endif

                if(algorithm_enable_)
                {
                    // 发布前 走补点算法
                    int mode = 1;
                    if(packet.data_block[0].echo_type != packet.data_block[1].echo_type)
                    {
                      mode  = Decoder<T_PointCloud>::param_.publish_mode + 1;
                    }
                    else
                    {
                      mode = packet.data_block[0].echo_type;
                    }
                    Filterate::GetInstance()->filter(*m_qmap_one_circle_point_, *m_qmap_pcap_one_circle_point_,mode);
                }

                OnePointInfoManage* tmp = nullptr;
                tmp = m_qmap_pcap_one_circle_point_;
                m_qmap_pcap_one_circle_point_ = m_qmap_one_circle_point_;
                m_qmap_one_circle_point_ = tmp;
                tmp = nullptr;

                // 将m_qMapOneCirclePoint数据转为ros发布结构体
                for(int i = 0; i < m_qmap_one_circle_point_->index_record_size_;i++)
                {
                    OnePointInfo& tem = m_qmap_one_circle_point_->GetDataIt(i);

                    if(outlier_enable_)
                    {
                        if(tem.non_filterable == 0 && (tem.distance > Filterate::GetInstance()->GetFullSearchScope()))
                        {
                             tem.x = 0;
                             tem.y = 0;
                             tem.z = 0;
                             tem.distance = 0;
                            //continue; // 不知道为啥 这个程序架构不允许 continue 否则发布点云的时候会报错
                        }                            
                    }
                    if(close_range_outliers_enable_)
                    {
                        if(tem.close_range_outliers == 0 && (tem.distance < Filterate::GetInstance()->GetFullSearchScope()))
                        {
                             tem.x = 0;
                             tem.y = 0;
                             tem.z = 0;
                             tem.distance = 0;
                            //continue;
                        }
                    }
                   
                    typename T_PointCloud::PointT point;
                    setX(point, tem.x);
                    setY(point, tem.y);
                    setZ(point, tem.z);

                    setIntensity(point, tem.intensity);
                    setTimestamp(point, tem.time);
                    setRing(point, tem.line_id);
                    
                    this->point_cloud_->points.emplace_back(std::move(point));
                    this->last_point_ts_ = tem.time;
                }

                compulsion_publish = false;
                cur_circle_frame_num_publish = 0;
                this->cb_split_frame_(144, this->cloudTs());
                this->first_point_ts_ = pkt_ts;

                m_qmap_one_circle_point_->Clear();
                ret = true;
            }
            pre_angle = h_angle;

            for(int j = 0; j < 48;j++)
            {
                int l_line = j+1;
                int l_line_id;
                int h_angle_temp = h_angle; // 由于在for循环中 17-32线需要+38 因此增加临时变量 获取当前水平角度
                int zero_h_angle_temp = h_angle; // 从0开始算的水平角度
                float distance = packet.data_block[i].channel_data[j].distance << 2;
                distance/=1000;

                int l_i = j % 8;
                int l_j = j / 8;
                int strong_weak =  (packet.data_block[i].strong_weak_flag[l_j] & (0x01 << l_i)) > 0?1:2;

                if(strong_weak == 1)
                {
                    // 4.0米内禁止出强光
                    if(distance <= 4.0) 
                    {
                        distance = 0;
                    }
                }

                if(l_line >16 && l_line <= 32)
                {
                    h_angle_temp += 38; // 3.8度的偏移
                }
                // 水平发光时序补偿
                int h_lum_timing_comp =  (((h_angle_lum_comp_[j] - 1) * h_angle_1_6us_));
                double points_time = pkt_ts + ((h_angle_lum_comp_[j] - 1) * 1.6) * 1e-6 ;

                h_angle_temp = (int)(((h_angle_temp*100 + h_compensate_angle ) + h_lum_timing_comp )+ 360000)%360000;//获取水平角 精度0.01

                switch(rotating_mirror)
                {
                case 0:
                    l_line_id = (l_line - 1)*3+rotating_mirror + 2;
                    break;
                case 1:
                    l_line_id = (l_line - 1)*3+rotating_mirror + 0;
                    break;
                case 2:
                    l_line_id = (l_line - 1)*3+rotating_mirror + 1;
                    break;
                }

                //int l_iomdex = this->chan_angles_.vertAdjust(j);
                int l_iomdex = this->chan_angles_.horiz_vertAdjust(l_line_id - 1, h_angle);
                l_iomdex = (int)(l_iomdex + 360000) % 360000;

                int32_t azimuth_index = h_angle_temp;
                int32_t verticalVal_740 = l_iomdex;
                //WJ_INFO << "l_line_id" << (l_line_id-1) << "    h_angle" << h_angle << "    vangle:" << verticalVal_740 << WJ_REND;
                float xy,x,y,z;


                xy = distance * COS(verticalVal_740);
                x = xy * (COS(azimuth_index));
                y = xy * SIN(azimuth_index);
                z = distance * SIN(verticalVal_740);
                this->transformPoint(x, y, z);

                OnePointInfo one_point_info_tmp;
                one_point_info_tmp.time = points_time;
                one_point_info_tmp.rotating_mirror_id = rotating_mirror;
                one_point_info_tmp.ld_id = l_line;
                // 重新映射ros反射率颜色 将 255 -> 0 , 0 -> 255  ,255 的最大值修改至 0.75 目的是 跳过ros紫色区域
                // 255 - 0 颜色依次为 深蓝-浅蓝-绿色-黄色-红色-紫色 -因此将255再次映射到145 防止出现紫色
                one_point_info_tmp.intensity = (255-packet.data_block[i].channel_data[j].intensity) * 0.75;
                one_point_info_tmp.low_thres_pulsewidth = packet.data_block[i].channel_data[j].low_thres_pulse_width;
                one_point_info_tmp.high_thres_pulsewidth = packet.data_block[i].channel_data[j].high_thres_pulse_width;
                one_point_info_tmp.old_distance = distance * 1000;
                one_point_info_tmp.distance = distance * 1000;
                one_point_info_tmp.azimuth = h_angle_temp / 10; // 精度0.01
                one_point_info_tmp.vazimuth = l_iomdex;
                one_point_info_tmp.h_angle = packet.data_block[i].horizontal_angle;
                one_point_info_tmp.h_angle = (int)(one_point_info_tmp.h_angle + 3600)%3600;//获取水平角 精度0.01
                one_point_info_tmp.v_angle = this->chan_angles_.horiz_vertAdjust(l_line_id - 1, h_angle);//Cfg.opticalPathAngle_Line[l_line_id - 1].v_angle;
                one_point_info_tmp.echo_type = echo_type; // 1- 最强 2-最后 // 主要用于显示在表中
                one_point_info_tmp.strong_weak = strong_weak;
                one_point_info_tmp.y = y;//新坐标系计算方式，逆时针改顺时针的方式
                one_point_info_tmp.x = x;
                one_point_info_tmp.z = z;//不补偿
                one_point_info_tmp.line_id = l_line_id;//3*(l_line-1)+rotating_mirror+1;
#if POINTS_COMPENSE && !POINTCLOUD_COMPENSE && SINGLE_POINT_COMPENSE && !PKG_POINT_COMPENSE
                pointCompense(&one_point_info_tmp, quaternion, pos_cur);
#endif
                one_point_info_tmp.index = GetOnePointInfoKey(one_point_info_tmp);
                m_qmap_one_circle_point_->insert(one_point_info_tmp.index,std::move(one_point_info_tmp));               
            }
        }       

#if POINTS_COMPENSE && !POINTCLOUD_COMPENSE && !SINGLE_POINT_COMPENSE && PKG_POINT_COMPENSE
        pkgPointsCompense(*m_qmap_one_circle_point_, quaternion, pos_cur, 4);
#endif  

        return ret;     
    }
#endif

    template <typename T_PointCloud>
    void DecoderVanjee740<T_PointCloud>::processDifopPkt(std::shared_ptr<ProtocolBase> protocol)
    {
      std::shared_ptr<CmdClass> sp_cmd = std::make_shared<CmdClass>(protocol->MainCmd,protocol->SubCmd);
      std::shared_ptr<ProtocolAbstract> p;

      if(p.get() == nullptr)
        return;

    }

} 
   
} 
